#include "seekNodeWay.h"
#include "control.h"
#include "C_code_Log.h"
#include "camera.h"
#include "photoelectric.h"
#include "gyro.h"
#include "Mydelay.h"
/**

* 函数介绍：默认找点(近距离找点)
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：摄像头检测横段白线
* 作者    ：

*/
uint8_t seekNodeMethod_default() {
  static uint8_t flag = 0;
  if (flag == 0) {
    camera_SendCommand(CAMERA_CMD_SHORT_FIND_NODE);
    flag = 1;
  }
  uint8_t result = getIsFindNode();
  if (result == 1) {
    flag = 0;
    camera_SendCommand(CAMERA_CMD_FIND_LINE);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
  }
  return result;
}

/**

* 函数介绍：中距离找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：摄像头检测横段白线
* 作者    ：

*/
uint8_t seekNodeMethod_middle() {
  static uint8_t flag = 0;
  if (flag == 0) {
    camera_SendCommand(CAMERA_CMD_MIDDLE_FIND_NODE);
    flag = 1;
  }
  uint8_t result = getIsFindNode();
  if (result == 1) {
    flag = 0;
    camera_SendCommand(CAMERA_CMD_FIND_LINE);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
  }
  return result;
}

/**

* 函数介绍：远距离找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：摄像头检测横段白线
* 作者    ：

*/
uint8_t seekNodeMethod_remote() {
  static uint8_t flag = 0;
  if (flag == 0) {
    camera_SendCommand(CAMERA_CMD_REMOTE_FIND_NODE);
    flag = 1;
  }
  uint8_t result = getIsFindNode();
  if (result == 1) {
    flag = 0;
    camera_SendCommand(CAMERA_CMD_FIND_LINE);
    HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
  }
  return result;
}

/**
 * @brief 延时找点
 * 
 * @param time 延时的时间，单位：ms
 * @return uint8_t 
 */
uint8_t seekNodeMethod_defaultDelay(uint16_t time){
    static uint8_t flag = 0;
    if(flag == 0&&seekNodeMethod_default()==1){
        flag = 1;
    }
    if(flag==1&&myDelay((uint32_t)seekNodeMethod_defaultDelay,time)==1){
        flag = 0;
        return 1;
    }
    return 0;
}
/**

* 函数介绍：左光电找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：左光电触发白线
* 作者    ：

*/
uint8_t seekNodeMethod_pesL(){
	static uint8_t flag=0;
	if(flag == 0 && PES_LEFT == 0)
	{
		flag = 0;
        return 1;
	}
	return 0;
}
/**

* 函数介绍：右光电找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：右光电触发白线
* 作者    ：

*/
uint8_t seekNodeMethod_pesR(){
	static uint8_t flag=0;
	if(flag == 0 && PES_RIGHT == 0)
	{
		flag = 0;
        return 1;
	}
	return 0;
}
/**

* 函数介绍：尾光电找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：尾光电触发
* 作者    ：

*/
uint8_t seekNodeMethod_pesB(){
	static uint8_t flag=0;
	if(flag == 0 && PES_BACK == 0)
	{
		flag = 0;
        return 1;
	}
	return 0;
}
/**

* 函数介绍：前光电找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：尾光电触发
* 作者    ：

*/
uint8_t seekNodeMethod_pesF(){
	static uint8_t flag=0;
	if(flag == 0 && PES_FRONT == 0)
	{
		flag = 0;
        return 1;
	}
	return 0;
}
/**

* 函数介绍：上平台找点
* 输入参数：无
* 输出参数：无
* 返回值  ：
* 其他    ：陀螺仪俯仰角检测状态
* 作者    ：

*/
uint8_t seekNodeMethod_Platform() {
  static uint8_t flag = 0;
  static FindLine save;
  //平台前的上坡，头上去后把寻线改为默认
  if (flag == 0 && gyro_getPitch() < -8) {
    gl_controlCenter.findLineWay = FL_UP_LONG;
    gl_controlCenter.findLineFlag = 0;
    flag = 1;
  }
  //在坡上时，摄像头已经看不到线了，寻线改为按陀螺仪
  if (flag == 1 && inRange((int)gyro_getPitch(), -16, -30)) {
    gl_controlCenter.findLineWay = NFL_GYRO_UP_PLATFORM;
    gl_controlCenter.findLineFlag = 0;
    flag = 2;
  }
  if (flag == 2 && gyro_getPitch() > -5) {
    flag = 0;
    return 1;
  }
  if (PES_FRONT == 0)
  {
    LOG_INFO("查找平台不正常");
    flag = 0;
    return 1;
  }
  LOG_INFO("seekNodeMethod_Platform");
  return 0;
}

/// @brief 寻找节点任务回调函数
/// @param control 控制台
/// @note 在完成寻找节点任务后调用，将寻找节点任务状态设置为完成，将停车任务状态置为准备就绪
static void seekNodeCallback(ControlCenterTypeDef *control){
    control->runState->seekNodeState = MY_EXIT;
    control->runState->carParkState = MY_READY;
}

/// @brief  寻找节点任务处理函数
/// @param control 控制台
/// @note 在寻找节点任务状态为准备就绪时调用，根据寻找节点任务的类型执行不同的寻找节点任务
void seekNodeHandle_Task(ControlCenterTypeDef *control){
    int result = 0;
    // 如果寻找节点任务状态为准备就绪
    if(control->runState->seekNodeState == MY_READY){

        // 根据寻找节点任务的类型执行不同的寻找节点任务，sectionInfo是当前路段的信息的指针，指向地图
        switch (control->sectionInfo->seekNodeWay) {
          case SEEK_default:
            result = seekNodeMethod_default();
            break;
          case SEEK_default_200:
            result = seekNodeMethod_defaultDelay(200);
            break;
          case SEEK_default_400:
            result = seekNodeMethod_defaultDelay(400);
            break;
          case SEEK_middle:
            result = seekNodeMethod_middle();
            break;
          case SEEK_remote:
            result = seekNodeMethod_remote();
            break;
          case SEEK_PesL:
            result = seekNodeMethod_pesL();
            break;
          case SEEK_PesR:
            result = seekNodeMethod_pesR();
            break;
          case SEEK_PesB:
            result = seekNodeMethod_pesB();
            break;
          case SEEK_PesF:
            result = seekNodeMethod_pesF();
            break;
          case SEEK_Platform:
            result = seekNodeMethod_Platform();
            break;
          case SEEK_NONE:
            result = 1;
            break;
          default:
            result = 1;
            break;
        }
        if (result == 1) {
          seekNodeCallback(control);
        }
    }
}